//
//  BoWWrapper.cpp
//  BoW
//
//  Created by CADCG on 16/4/23.
//  Copyright © 2016年 CADCG. All rights reserved.
//

#include "BoWWrapper.hpp"
#include "global.h"
using namespace cv;
using namespace std;
void BoWWrapperORB::addImages(const std::vector<std::vector<cv::KeyPoint> > &kpts, const std::vector<cv::Mat> &desps)
{
  m.lock();
    for (int i=0; i<kpts.size(); ++i) {
        allKeyPoints.push_back(kpts[i]);
    }
  allDesp.resize(desps.size());
  for (int i=0; i<desps.size(); ++i)
  {
    allDesp.push_back(vector<cv::Mat> ());
    auto &d = allDesp.back();
    auto &desp = desps[i];
    for (int j=0; j<desp.rows; ++j)
    {
      d.push_back(desp.row(j).clone());
    }
  }
  m.unlock();
}

//QueryResult BoWWrapperORB::queryByDirectMatching(const vector<KeyPoint> &kpts,const cv::Mat& desp)
//{
//    m.lock();
//    const int TH_LOW = 50;
//    const float mfNNratio=0.8;
//    const float BoWRatio = 0.8;
//    assert(isTrained);
//    vector<cv::Mat> despVec;
//    for (int i=0; i<desp.rows; ++i) {
//        despVec.push_back(desp.row(i));
//    }
//    DBoW2::BowVector v;
//    DBoW2::FeatureVector fv;
//    std::vector<double> bowScores;
//    voc.transform(despVec, v, fv, 4);
//    double maxScore = std::numeric_limits<double>::min();
//    vector<double> vScores;
//    vector<size_t> vCandidates;
//    vector<size_t> vNMatches;
//    //get Image candidate list(Bow score is greater than 0.8 maxScore)
//    for (int i=0; i<vBowVectors.size(); ++i) {
//        double score;
//        score = voc.score(v, vBowVectors[i]);
//        bowScores.push_back(score);
//        if (score>maxScore){
//            maxScore = score;
//        }
//    }
//    for (size_t i=0; i<bowScores.size(); ++i) {
//        if (bowScores[i]>BoWRatio*maxScore) {
//            vCandidates.push_back(i);
//        }
//    }
//    //never go into there
//    if (vCandidates.empty()) {
//        
//    }
//    size_t maxNMatches = 0;
//    std::vector<cv::Matx33d> Htqs;
//    std::vector<int> ids;
//    //perform orb matching with candidates
//    for (int i=0; i<vCandidates.size(); ++i) {
//        size_t id = vCandidates[i];
//        markerAR::Marker* nowMarker = markerAR::map->getMarker(id);
//        markerAR::binaryMatcher->knnMatch(desp, nowMarker->orbStructure->desp, <#vector<vector<cv::DMatch> > &matches#>, <#int k#>)
//        
//        std::vector<bool> vbMatches(desp.rows,false);
//        size_t nmatches = 0;
//        std::vector<Point2f> vkN,vkD;
//        while(nfitb!= nfite && dfitb != dfite){
//            if (nfitb->first == dfitb->first){
//                const vector<unsigned int> vIndicesD = dfitb->second;
//                const vector<unsigned int> vIndicesF = nfitb->second;
//                
//                for(size_t iD=0; iD<vIndicesD.size(); iD++){
//                    const unsigned int realIdxD = vIndicesD[iD];
//                    
//                    const cv::Mat &dD= allDesp[id][realIdxD];
//                    
//                    int bestDist1=256;
//                    int bestIdxF =-1 ;
//                    int bestDist2=256;
//                    
//                    for(size_t iF=0; iF<vIndicesF.size(); iF++){
//                        const unsigned int realIdxF = vIndicesF[iF];
//                        
//                        const cv::Mat &dF = desp.row(realIdxF);
//                        
//                        const int dist =  DBoW2::FORB::distance(dD,dF);
//                        
//                        if(dist<bestDist1){
//                            bestDist2=bestDist1;
//                            bestDist1=dist;
//                            bestIdxF=realIdxF;
//                        }
//                        else if(dist<bestDist2){
//                            bestDist2=dist;
//                        }
//                    }
//                    
//                    if(bestDist1<=TH_LOW){
//                        vkD.push_back(allKeyPoints[id][realIdxD].pt);
//                        vkN.push_back(kpts[bestIdxF].pt);
//                    }
//                    
//                }
//                
//                nfitb++;
//                dfitb++;
//            }
//            else if(nfitb->first <dfitb->first){
//                nfitb = fv.lower_bound(dfitb->first);
//            }
//            else{
//                dfitb = vFeatureVectors[id].lower_bound(nfitb->first);
//            }
//        }
//        Mat mask;
//        if (vkN.size()<8||vkD.size()<8) {
//            continue;
//        }
//        cv::Mat Htq = findHomography(vkN, vkD, mask,CV_RANSAC,5);
//        //        cv::Matx33d Htq = findHomographyRHO(vkN, vkD, mask,CV_RANSAC,5);
//        
//        Htqs.push_back(Htq);
//        nmatches = cv:: norm(mask,cv::NORM_L1);
//        if (nmatches>maxNMatches) {
//            maxNMatches = nmatches;
//        }
//        vNMatches.push_back(nmatches);
//        ids.push_back(id);
//    }
//    if (maxNMatches<15) {
//        m.unlock();
//        return QueryResult();
//    }
//    QueryResult ret;
//    cout<<"maxNMatches"<<maxNMatches<<endl;
//    
//    for (int i=0; i<vNMatches.size(); ++i) {
//        if (vNMatches[i]>(float)maxNMatches*0.5) {
//            _QueryResult res;
//            res.id = ids[i];
//            res.nMatches = vNMatches[i];
//            res.Htq = Htqs[i];
//            ret.push_back(res);
//        }
//    }
//    sort(ret.begin(), ret.end(), [](const _QueryResult&a,const _QueryResult b){return a.nMatches>b.nMatches;});
//    m.unlock();
//    return ret;
//}

QueryResult BoWWrapperORB::query(const vector<KeyPoint> &kpts,const cv::Mat& desp)
{
    
    m.lock();
    const int TH_LOW = 50;
    const float mfNNratio=0.8;
    const float BoWRatio = 0.8;
    assert(isTrained);
    vector<cv::Mat> despVec;
    for (int i=0; i<desp.rows; ++i) {
        despVec.push_back(desp.row(i));
    }
    DBoW2::BowVector v;
    DBoW2::FeatureVector fv;
    std::vector<double> bowScores;
    voc.transform(despVec, v, fv, 4);
    double maxScore = std::numeric_limits<double>::min();
    vector<double> vScores;
    vector<size_t> vCandidates;
    vector<size_t> vNMatches;
    //get Image candidate list(Bow score is greater than 0.8 maxScore)
    for (int i=0; i<vBowVectors.size(); ++i) {
        double score;
        score = voc.score(v, vBowVectors[i]);
        bowScores.push_back(score);
        if (score>maxScore){
            maxScore = score;
        }
    }
    for (size_t i=0; i<bowScores.size(); ++i) {
        if (bowScores[i]>BoWRatio*maxScore) {
            vCandidates.push_back(i);
        }
    }
    //never go into there
    if (vCandidates.empty()) {
        
    }
    size_t maxNMatches = 0;
    std::vector<cv::Matx33d> Htqs;
    std::vector<int> ids;
    //perform orb matching with candidates
    for (int i=0; i<vCandidates.size(); ++i) {
        size_t id = vCandidates[i];
         DBoW2::FeatureVector::const_iterator nfitb = fv.begin();
         DBoW2::FeatureVector::const_iterator dfitb = vFeatureVectors[id].begin();
         DBoW2::FeatureVector::const_iterator nfite = fv.end();
         DBoW2::FeatureVector::const_iterator dfite = vFeatureVectors[id].end();
        std::vector<bool> vbMatches(desp.rows,false);
        size_t nmatches = 0;
        std::vector<Point2f> vkN,vkD;
        while(nfitb!= nfite && dfitb != dfite){
            if (nfitb->first == dfitb->first){
                const vector<unsigned int> vIndicesD = dfitb->second;
                const vector<unsigned int> vIndicesF = nfitb->second;
                
                for(size_t iD=0; iD<vIndicesD.size(); iD++){
                    const unsigned int realIdxD = vIndicesD[iD];
                    
                    const cv::Mat &dD= allDesp[id][realIdxD];
                    
                    int bestDist1=256;
                    int bestIdxF =-1 ;
                    int bestDist2=256;
                    
                    for(size_t iF=0; iF<vIndicesF.size(); iF++){
                        const unsigned int realIdxF = vIndicesF[iF];
                          
                        const cv::Mat &dF = desp.row(realIdxF);
                        
                        const int dist =  DBoW2::FORB::distance(dD,dF);
                        
                        if(dist<bestDist1){
                            bestDist2=bestDist1;
                            bestDist1=dist;
                            bestIdxF=realIdxF;
                        }
                        else if(dist<bestDist2){
                            bestDist2=dist;
                        }
                    }
                    
                    if(bestDist1<=TH_LOW){
                        vkD.push_back(allKeyPoints[id][realIdxD].pt);
                        vkN.push_back(kpts[bestIdxF].pt);
                    }
                    
                }
                
                nfitb++;
                dfitb++;
            }
            else if(nfitb->first <dfitb->first){
                nfitb = fv.lower_bound(dfitb->first);
            }
            else{
                dfitb = vFeatureVectors[id].lower_bound(nfitb->first);
            }
        }
        Mat mask;
        if (vkN.size()<8||vkD.size()<8) {
            continue;
        }
        cv::Mat Htq = findHomography(vkN, vkD, mask,CV_RANSAC,5);
//        cv::Matx33d Htq = findHomographyRHO(vkN, vkD, mask,CV_RANSAC,5);

        Htqs.push_back(Htq);
         nmatches = cv:: norm(mask,cv::NORM_L1);
        if (nmatches>maxNMatches) {
            maxNMatches = nmatches;
        }
        vNMatches.push_back(nmatches);
        ids.push_back(id);
    }
    if (maxNMatches<15) {
        m.unlock();
        return QueryResult();
    }
    QueryResult ret;
    cout<<"maxNMatches"<<maxNMatches<<endl;
    
    for (int i=0; i<vNMatches.size(); ++i) {
        if (vNMatches[i]>(float)maxNMatches*0.5) {
            _QueryResult res;
            res.id = ids[i];
            res.nMatches = vNMatches[i];
            res.Htq = Htqs[i];
            ret.push_back(res);
        }
    }
    sort(ret.begin(), ret.end(), [](const _QueryResult&a,const _QueryResult b){return a.nMatches>b.nMatches;});
    m.unlock();
    return ret;
}

void BoWWrapperORB::addImage(const std::vector<cv::KeyPoint> &kpts, const cv::Mat &desp)
{
  m.lock();
    allKeyPoints.push_back(kpts);
  allDesp.push_back(vector<cv::Mat> ());
  auto &d = allDesp.back();
  for (int j=0; j<desp.rows; ++j)
  {
    d.push_back(desp.row(j).clone());
  }
  m.unlock();
}


void BoWWrapperORB::toDespVec(const cv::Mat&desp,std::vector<cv::Mat>&despVec)
{
    despVec.clear();
    for (int i=0; i<desp.rows; ++i) {
        despVec.push_back(desp.row(i));
    }
}

void BoWWrapperORB::train()
{
  
  m.lock();
  voc.create(allDesp);
  DBoW2::BowVector v;
  DBoW2::FeatureVector fv;
    vBowVectors.clear();
    vFeatureVectors.clear();
  for (int i=0; i<allDesp.size(); ++i)
  {
      voc.transform(allDesp[i], v,fv,4);
      vBowVectors.push_back(v);
      vFeatureVectors.push_back(fv);
  }
  isTrained = true;
  m.unlock();
}

void BoWWrapperORB::clear()
{
  m.lock();
  allDesp.clear();
  isTrained = false;
  
  m.unlock();
}

